#include "WPILib.h"
#include "Robot2489.h"

//To-ask: Watchdog? Gyro progress?

void Robot2489::OperatorControl(void) {
	myRobot->SetSafetyEnabled(true);
	
	float strafex;
	float strafey;
	float spinx;
	float crouch;
	
	while (IsOperatorControl()) 	
	{
		/* For using the game controller instead of joysticks
		strafex = game->GetRawAxis(1);
		strafey = game->GetRawAxis(2);
		spinx = -game->GetRawAxis(3);
		crouch = 1;
		 */
		
		ReadIOModule();
		
		/*
		//Emergency shutdown
		if (EmergencyPause){
			StopArm();
			//SpinningWheelStop();
			ManipStop();
			myRobot->MecanumDrive_Cartesian(0, 0, 0);
			continue;
		}
		*/
		strafex = -strafeStick->GetX();
		strafey = -strafeStick->GetY();
		spinx = -spinStick->GetX();
		
		//if a button is pressed, slow down the movement for more fine controls
		if (!Slow)
			crouch = 2;
		else
			crouch = 1;
	
		//read the angle of the gyro
		//the angle is used to maintain the orientation of the robot if it drifts
		//as of this moment, we have not been able to incorporate it into mecanum drive
		//commented out the io module until we get it to communicate with the 
		//ReadIOModule();
		
		//commented out the code involving the manipulator limit switches until they are 
		//Manipulator();
		//screen->PrintfLine(DriverStationLCD::kUser_Line1, "Peg Height = %d", pegHeight);
		//screen->UpdateLCD();
		
		//dead zone
		if (strafex<0.05 && strafex>-0.05)
			strafex=0;
		if (strafey<0.05 && strafey>-0.05)
			strafey=0;
		if (spinx<0.05 && spinx>-0.05)
			spinx=0;
		
		//"crouch" function that slows down the robot by a certain factor based on the Z axis
		strafex=strafex/crouch;
  		strafey=strafey/crouch;
  		spinx=spinx/crouch;
			
 		myRobot->MecanumDrive_Cartesian(strafex, strafey, spinx/*gyro->GetAngle()*/);
 		
 		//currently, the motors powering the winch on the are reversed by nature (mechanically)
 		//there is no need to reverse the motors in the program
 		if (manipulator1->GetRawButton(6)) {
 			ManipUp();
 			screen->PrintfLine(DriverStationLCD::kUser_Line5, "Manip up");
 		}
 		else if (manipulator1->GetRawButton(7)) {
 			ManipDown();
 			screen->PrintfLine(DriverStationLCD::kUser_Line5, "Manip down");
 		}
 		else {
 			ManipStop();
 			screen->PrintfLine(DriverStationLCD::kUser_Line5, "Manip off");
 		}
 		/*
 		if (manipulator1->GetRawButton(4)) {
 			SpinningWheelIn();
 			screen->PrintfLine(DriverStationLCD::kUser_Line6, "Wheel in");
 		}
 		else if (manipulator1->GetRawButton(5))
 		{
 			SpinningWheelOut();
 			screen->PrintfLine(DriverStationLCD::kUser_Line6, "Wheel out");
 		}
 		else if (manipulator1->GetRawButton(10))
 		{
 			SpinningWheelRotateDown();
 			screen->PrintfLine(DriverStationLCD::kUser_Line6, "Wheel rotate down");
 		}
 		else if (manipulator1->GetRawButton(11))
 		{
 			SpinningWheelRotateUp();
 			screen->PrintfLine(DriverStationLCD::kUser_Line6, "Wheel rotate up");
 		}
 		else {
 			SpinningWheelStop();
 			screen->PrintfLine(DriverStationLCD::kUser_Line6, "Wheel off");
 		}
 		*/
 		if (manipulator1->GetRawButton(3)){
 			ArmUp();
 			screen->PrintfLine(DriverStationLCD::kUser_Line4, "Arm up");
 		}
 		else if (manipulator1->GetRawButton(2))
 		{
 			ArmDown();
 			screen->PrintfLine(DriverStationLCD::kUser_Line4, "Arm down");
 		}
 		else {
 			StopArm();
 			screen->PrintfLine(DriverStationLCD::kUser_Line4, "Arm not moving");
 		}
 		/*
 		if (dseio->GetDigital(15)) {
 			deployer->Set(1.0);
 			screen->PrintfLine(DriverStationLCD::kUser_Line3, "That was easy");
 			screen->PrintfLine(DriverStationLCD::kUser_Line2, "Servo pos: %f", deployer->Get());
 		}*/
 		
		Wait(0.005); // wait for a motor update time
	}
}
 
//START_ROBOT_CLASS(Robot2489);
